"""
Author: Morphlng
Date: 2024-03-19 14:47:00
LastEditTime: 2024-03-19 14:47:00
LastEditors: Morphlng
Description: Test for bounding box
FilePath: /DrivingGym/tests/common/geom/boundingbox_test.py
"""

import math

import numpy as np

from driving_gym.simulation.common.geom import (
    BoundingBox,
    Location,
    Transform,
    Vector3D,
)
from driving_gym.simulation.common.stub import try_import_carla

carla = try_import_carla()


class TestBoundingBox:

    def test_initialization(self):
        loc = Location(1, 2, 3)
        extent = Vector3D(1, 1, 1)
        bbox = BoundingBox(location=loc, extent=extent)
        assert (
            bbox.location == loc and bbox.extent == extent
        ), "Initialization of BoundingBox failed."

    def test_contains_point_true(self):
        loc = Location(0, 0, 0)
        extent = Vector3D(1, 1, 1)
        bbox = BoundingBox(location=loc, extent=extent)
        transform = Transform(location=Location(0, 0, 0))
        point = Location(0.5, 0.5, 0.5)
        assert bbox.contains(point, transform), "BoundingBox should contain the point."

    def test_contains_point_false(self):
        loc = Location(0, 0, 0)
        extent = Vector3D(1, 1, 1)
        bbox = BoundingBox(location=loc, extent=extent)
        transform = Transform(location=Location(0, 0, 0))
        point = Location(2, 2, 2)
        assert not bbox.contains(
            point, transform
        ), "BoundingBox should not contain the point."

    def test_as_numpy_array_bottom_only(self):
        loc = Location(0, 0, 0)
        extent = Vector3D(1, 1, 1)
        bbox = BoundingBox(location=loc, extent=extent)
        array = bbox.as_numpy_array(bottom_only=True)
        assert array.shape == (4, 3), "Array shape incorrect for bottom_only vertices."

    def test_as_numpy_array_full(self):
        loc = Location(0, 0, 0)
        extent = Vector3D(1, 1, 1)
        bbox = BoundingBox(location=loc, extent=extent)
        array = bbox.as_numpy_array()
        assert array.shape == (
            8,
            3,
        ), "Array shape incorrect for full bounding box vertices."

    def test_transform_to_world_frame_bottom_only(self):
        loc = Location(0, 0, 0)
        extent = Vector3D(1, 1, 1)
        bbox = BoundingBox(location=loc, extent=extent)
        transform = Transform(location=Location(0, 0, 0))
        world_cords = bbox.transform_to_world_frame(transform, bottom_only=True)
        assert world_cords.shape == (
            4,
            3,
        ), "World coordinates shape incorrect for bottom_only vertices."

    def test_transform_to_world_frame_full(self):
        loc = Location(0, 0, 0)
        extent = Vector3D(1, 1, 1)
        bbox = BoundingBox(location=loc, extent=extent)
        transform = Transform(location=Location(0, 0, 0))
        world_cords = bbox.transform_to_world_frame(transform)
        assert world_cords.shape == (
            8,
            3,
        ), "World coordinates shape incorrect for full bounding box vertices."
